# ----------------------------
# coding: utf-8
# 开发作者 : 钱嘉龙
# 创建日期 : 2022/2/24 13:41
# ----------------------------
from Visual_Position_Lib import Camera
import cv2 as cv
import numpy as np
import math


def camera_process_2():
    base_point = [330, 90]
    red_lower = np.array([[0, 50, 110], [140, 100, 100]])
    red_upper = np.array([[4, 255, 255], [180, 255, 255]])
    # 红绿蓝
    lower = np.array([red_lower[0], [40, 100, 100], [90, 150, 150]])
    upper = np.array([red_upper[0], [80, 255, 255], [125, 255, 255]])

    cap = cv.VideoCapture(0)
    while True:
        right_or_left = 0
        front_or_behind = 0
        center = []
        distance = 0

        # 获取摄像头图像并获取相关区域
        ret, image = cap.read()
        if not ret:
            print('No Camera')
            continue


        # image = cv.imread('F:\\EngineeringDocuments\\Logistics_Handling_Robot\\images\\blue.jpg')
        image = cv.resize(image, (640, 480))

        # 调整亮度以及滤波
        # image_brightness = cam.contrast_brightness(image, 1, 0)
        # blurred = cam.gaussian_blur(image, 5)
        # gary = cam.convert_gray_scale(blurred)
        # binary = cam.threshold(gary)

        # RGB转HSV
        hsv_image = cv.cvtColor(image, cv.COLOR_BGR2HSV)
        dst_image = np.zeros_like(hsv_image)

        mask_red = cv.inRange(hsv_image, red_lower[1], red_upper[1])
        print('---------------------------------------------')
        for i in range(0, 3):
            mask = cv.inRange(hsv_image, lower[i], upper[i])
            if i == 0:
                mask = cv.bitwise_or(mask_red, mask)
            # if i == 1:
            #     cam_2.show_image('mask_', mask, 0, False)
            # 对获取的mask进行形态学操作
            # morphology_image = cam.morphology_process(mask, 0, kernel_rect_3, 1)
            # morphology_image = cam.morphology_process(morphology_image, 1, kernel_rect_3, 1)

            output = np.zeros((mask.shape[0], mask.shape[1], 3), np.uint8)
            num_labels, labels, stats, centroids = cv.connectedComponentsWithStats(mask, connectivity=4)
            point_num = -1
            for index, stat in enumerate(stats):
                # print('area:', stat[4])
                if 8000 < stat[4] < 40000:
                    point_num = index
                    break
            if point_num == -1 or point_num == 0:
                continue
            # print('point_num:', point_num)
            # for j in range(1, num_labels):
            #     mask = labels == j
            #     output[:, :, 0][mask] = np.random.randint(0, 255)
            #     output[:, :, 1][mask] = np.random.randint(0, 255)
            #     output[:, :, 2][mask] = np.random.randint(0, 255)
            # cv2.imshow('0', output)
            # print(centroids[point_num])
            if centroids[point_num] is not None:
                # distance = cam_2.calculate_long(centroids[point_num], base_point)
                distance = math.pow((centroids[point_num][0] - base_point[0]), 2) + math.pow((centroids[point_num][1] - base_point[1]), 2)
                distance = math.sqrt(distance)

                if distance > 15:
                    # 小车向左: 1       小车向右: 2
                    right_or_left = 0
                    # 小车向前: 1       小车向后: 2
                    front_or_behind = 0
                    if centroids[point_num][0] < base_point[0]:
                        right_or_left = 1
                    else:
                        right_or_left = 2
                    if centroids[point_num][1] < base_point[1]:
                        front_or_behind = 1
                    else:
                        front_or_behind = 2
                    # right_or_left, front_or_behind = cam_2.pos_adjust(centroids[point_num], base_point)
            # print('center', centroids[point_num])
            print('i', i)

        print('right_or_left:', right_or_left, 'front_or_behind', front_or_behind)
        # print('distance', distance)
        # cam.show_image('dst_image', dst_image, 0, False)
        cam_2.camera_move_control(right_or_left, front_or_behind)
        # cv.waitKey(0)
        # Esc退出
        key = cv.waitKey(1)
        if key == 27:
            break


if __name__ == '__main__':
    cam_2 = Camera()
    camera_process_2()
    cv.destroyAllWindows()












